﻿/*
*          =RobotCube=
*
*          Version : v0.01
*          Latest modification : 2016年10月17日 15:31:26
*          Author: Elfits.Liu（刘志浩）
*          立项于 =2016年10月=
*          武汉理工大学信息工程学院 =智能制造团队=   
*/

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Text.RegularExpressions;
using ABB.Robotics.Controllers.IOSystemDomain;
using ABB.Robotics.Controllers.EventLogDomain;

//引入 ABB.Robotics.Controllers.PC.dll from PC SDK 6.00.01
using ABB.Robotics.Controllers;
using ABB.Robotics.Controllers.Discovery;
using ABB.Robotics.Controllers.RapidDomain;
using System.Timers;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.IO;
using System.Runtime.Serialization;
using System.Runtime.Serialization.Formatters.Binary;
using kinectSocketController;
using System.Reflection;

namespace RobotCube
{

    public partial class FormMain : Form //继承了Form
    {

        private NetworkScanner scanner = null;
        private Controller controller = null;
        private ABB.Robotics.Controllers.RapidDomain.Task[] tasks = null;
        private NetworkWatcher networkwatcher = null;
        private System.Timers.Timer timer1 = null;
        private System.Timers.Timer timer2 = null;
        public System.Timers.Timer timer3 = null;
        private Socket serverSocket = null;
        private System.Byte[] buffer = new System.Byte[1024];
        public GrabProcess grabprocess = null;
        private bool grab_ed;

        //private RobotWare myRobotWare = null;
        //private RobotWareCollection myRobotWareCollection = null;

        /*
            Name:  FormMain()
            Note:    此处初始化scanner要放在FormMain()中，与官方文档所说不同。
            Date：  2016年10月17日 15:23:35
            Author: Elfits.Liu
        */
        public FormMain()
        {
            InitializeComponent();
            this.scanner = new NetworkScanner();
            this.scanner.Scan();
            ControllerInfoCollection controllers = scanner.Controllers;
            ListViewItem item = null;
            this.timer1 = new System.Timers.Timer(1000);
            timer1.Elapsed += new System.Timers.ElapsedEventHandler(isTaskStart);


            this.timer2 = new System.Timers.Timer(500);
            timer2.Elapsed += new System.Timers.ElapsedEventHandler(ReadData);

            foreach (ControllerInfo controllerInfo in controllers)
            {
                item = new ListViewItem(controllerInfo.IPAddress.ToString());
                item.SubItems.Add(controllerInfo.Id);
                item.SubItems.Add(controllerInfo.Availability.ToString());
                item.SubItems.Add(controllerInfo.IsVirtual.ToString());
                item.SubItems.Add(controllerInfo.SystemName);
                item.SubItems.Add(controllerInfo.Version.ToString());
                item.SubItems.Add(controllerInfo.ControllerName);
                item.SubItems.Add(controllerInfo.RunLevel.ToString());
                item.SubItems.Add(controllerInfo.HostName.ToString());
                item.SubItems.Add(controllerInfo.MacAddress.ToString());
                this.listControllerView.Items.Add(item);
                item.Tag = controllerInfo;
            }


        }



        private void FormMain_Load(object sender, EventArgs e)
        {
            CheckForIllegalCrossThreadCalls = false;
            this.networkwatcher = new NetworkWatcher(scanner.Controllers);
            this.networkwatcher.Found += new EventHandler<NetworkWatcherEventArgs>(HandleFoundEvent);
            this.networkwatcher.Lost += new EventHandler<NetworkWatcherEventArgs>(HandleLostEvent);
            this.networkwatcher.EnableRaisingEvents = true;
            this.timer1.Enabled = true;
            this.timer1.AutoReset = true;

            this.grab_ed = false;
            //this.Invoke(new EventHandler<NetworkWatcherEventArgs>(AddControllerToListView));
            // new Object[] { (this,e) };
            /******************以下是探索实验********************/

            // 1. Controller object
            //ControllerInfo info = new ControllerInfo(new Guid("F7A66D4D-F705-42B3-9E01-7B49B651F852"));
            //Controller myController;
            //myController = ControllerFactory.CreateFrom(info);
            /*
            if (info == null)
            {
                richTextBox1.Text = "info = null";
            }
            richTextBox1.AppendText(info.IsVirtual.ToString() + '\n');
            richTextBox1.AppendText("AA" + '\n');
            for (int i = 50; i > 0; i--)
            {
                richTextBox1.AppendText("BB" + '\n');
            }
            */
            //richTextBox1.Text = info.IPAddress.ToString();
            //myController.OperatingModeChanged += new EventHandler<OperatingModeChangeEventArgs>(OperatingModeChanged);
            //myController.StateChanged += new EventHandler<StateChangedEventArgs>(StateChanged);
            //myController.ConnectionChanged += new EventHandler<ConnectionChangedEventArgs>(ConnectionChanged);

        }


        /*
            Name:  buttonStartRAPID_Click()
            Note:    双击执行RAPID程序
            Date：  2016年10月17日 15:24:37
            Author: Elfits.Liu
        */
        private void buttonStartRAPID_Click(object sender, EventArgs e)
        {
            try
            {
                if (controller.OperatingMode == ControllerOperatingMode.Auto)
                {
                    tasks = controller.Rapid.GetTasks();
                    using (Mastership m = Mastership.Request(controller.Rapid))//注意这个using语句，与释放权限相关
                    {
                        //Perform operation
                        tasks[0].Start();
                    }
                    MessageBox.Show("Execution started");
                }
                else
                {
                    MessageBox.Show("Automatic mode is required to start execution from a remote client.");
                }
            }
            catch (System.InvalidOperationException ex)
            {
                MessageBox.Show("Mastership is held by another client." + ex.Message);
            }
            catch (System.Exception ex)
            {
                MessageBox.Show("Unexpected error occurred: " + ex.Message);
            }
        }

        /*
            Name:  listControllerView_DoubleClick
            Note:    双击IP连接机器人控制器
            Date：  2016年10月17日 15:25:07
            Author: Elfits.Liu
        */
        private void listControllerView_DoubleClick(object sender, EventArgs e)
        {

            ListViewItem item = this.listControllerView.SelectedItems[0];
            if (item.Tag != null)
            {
                ControllerInfo controllerInfo = (ControllerInfo)item.Tag;
                if
                (controllerInfo.Availability == Availability.Available)
                {
                    if (this.controller != null)
                    {
                        this.controller.Logoff();
                        this.controller.Dispose();
                        this.controller = null;
                    }
                    this.controller = ControllerFactory.CreateFrom(controllerInfo);
                    this.controller.Logon(UserInfo.DefaultUser);
                    MessageBox.Show("已连接");

                    this.grabprocess = new GrabProcess(this.controller);

                    this.controller.EventLog.MessageWritten += new EventHandler<MessageWrittenEventArgs>(EventlogShow);
                    this.richTextBox1.AppendText("与控制器" + this.controller.SystemName.ToString() + "连接成功  " + System.DateTime.Now.ToString() + "\r\n");
                }
                else
                {
                    MessageBox.Show("Selected controller not available.");
                }
            }
        }


        /*
            Name:  HandleFoundEvent
            Note:    处理发现控制器事件
            Date：  2016年10月17日 15:25:07
            Author: Elfits.Liu
        */
        void HandleFoundEvent(object sender, NetworkWatcherEventArgs e)
        {
            this.Invoke(new
            EventHandler<NetworkWatcherEventArgs>(AddControllerToListView), new Object[] { this, e });
        }

        /*
            Name:  HandleLostEvent
            Note:    处理丢失控制器事件
            Date：  2016年10月17日 15:25:07
            Author: Elfits.Liu
        */
        void HandleLostEvent(object sender, NetworkWatcherEventArgs e)
        {
            this.Invoke(new
            EventHandler<NetworkWatcherEventArgs>(DeleteControllerFromListView),
            new Object[] { this, e });
        }
        private void DeleteControllerFromListView(object sender, NetworkWatcherEventArgs e)
        {
            ControllerInfo controllerInfo = e.Controller;
            for (int i = 0; i < listControllerView.Items.Count; i++)
            {
                ListViewItem item = listControllerView.Items[i];
                if (item.Tag.Equals(e.Controller))
                {
                    this.listControllerView.Items.Remove(item);
                }
            }
        }

        /*
            Name:  AddControllerToListView
            Note:    添加新控制器到列表
            Date：  2016年10月17日 15:25:07
            Author: 
        */
        private void AddControllerToListView(object sender, NetworkWatcherEventArgs e)
        {
            ControllerInfo controllerInfo = e.Controller;

            ListViewItem item = new ListViewItem(controllerInfo.IPAddress.ToString());
            item.SubItems.Add(controllerInfo.Id);
            item.SubItems.Add(controllerInfo.Availability.ToString());
            item.SubItems.Add(controllerInfo.IsVirtual.ToString());
            item.SubItems.Add(controllerInfo.SystemName);
            item.SubItems.Add(controllerInfo.Version.ToString());
            item.SubItems.Add(controllerInfo.ControllerName);
            item.SubItems.Add(controllerInfo.RunLevel.ToString());
            item.SubItems.Add(controllerInfo.HostName);
            item.SubItems.Add(controllerInfo.MacAddress);

            this.listControllerView.Items.Add(item);
            item.Tag = controllerInfo;
        }

        /*
           Name:  write_robTarge
           Note:  根据文本框输入的数值给RAPID robTarget变量写值
           Date：  2017年1月4日 14:12:07
           Author: YANKE
        */
        private void write_robTarget()
        {
            ABB.Robotics.Controllers.RapidDomain.RapidData rd = this.controller.Rapid
                   .GetRapidData("T_ROB1", "testABB", "wanted_pos");
            ABB.Robotics.Controllers.RapidDomain.RobTarget robTarget;
            if (rd.Value is ABB.Robotics.Controllers.RapidDomain.RobTarget)
            {
                try
                {
                    robTarget = (ABB.Robotics.Controllers.RapidDomain.RobTarget)rd.Value;

                    Pos rt_Pos = new Pos();
                    rt_Pos.FillFromString2("[" + this.XtextBox.Text.Trim() + "," + this.YtextBox.Text.Trim()
                     + "," + this.ZtextBox.Text.Trim() + "]");
                    robTarget.Trans = rt_Pos;

                    Orient rt_orient = new Orient();
                    rt_orient.FillFromString2("[" + this.q1textBox.Text.Trim() + "," + this.q2textBox.Text.Trim()
                     + "," + this.q3textBox.Text.Trim() + "," + this.q4textBox.Text.Trim() + "]");
                    //rt_orient.FillFromString2("[0.104731,-0.729492,0.669942,0.0897298]");
                    robTarget.Rot = rt_orient;

                    ConfData rt_confData = new ConfData();
                    rt_confData.FillFromString2("[" + this.cf1textBox.Text.Trim() + "," + this.cf4textBox.Text.Trim()
                     + "," + this.cf6textBox.Text.Trim() + "," + this.cfxtextBox.Text.Trim() + "]");
                    //rt_confData.FillFromString2("[1,-1,-1,0]");
                    robTarget.Robconf = rt_confData;

                    ExtJoint rt_extJoint = new ExtJoint();
                    rt_extJoint.FillFromString2("[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]");
                    robTarget.Extax = rt_extJoint;

                    Mastership master = Mastership.Request(this.controller.Rapid);
                    rd.Value = robTarget;
                    master.Dispose();
                }
                catch (RapidDataFormatException)
                {
                    MessageBox.Show("只能输入数字！");
                }
            }
        }

        /*
           Name:  write_robTarge（RobTarget robTarget）
           Note:  带参数写位置坐标值
           Date：  2017年1月5日 14:12:07
           Author: YANKE
        */
        private void write_robTarget(RobTarget robTarget)
        {
            ABB.Robotics.Controllers.RapidDomain.RapidData rd = this.controller.Rapid
                  .GetRapidData("T_ROB1", "Wobj_wobj0", "wanted_pos");
            if (rd.Value is ABB.Robotics.Controllers.RapidDomain.RobTarget)
            {

                Mastership master = Mastership.Request(this.controller.Rapid);
                rd.Value = robTarget;
                master.Dispose();
            }
        }

        /*
           Name:  check
           Note:  判断文本框是不是数字
           Date：  2017年1月4日 14:12:07
           Author: YANKE
        */
        private void check(TextBox textBox)
        {
            string pattern = "^-?([1-9]d*.d*|0.d*[1-9]d*|0?.0+|0)$";
            //string pattern = "^[1-9]d*$";
            Match match = Regex.Match(textBox.Text.Trim(), pattern);
            if (!match.Success)
            {
                textBox.SelectionStart = textBox.Text.Length;
                MessageBox.Show("只能输入数字！");
            }
        }

        /*
           Name:  set_signal
           Note:  判断文本框是不是数字
           Date：  2017年1月4日 14:12:07
           Author: YANKE
        */
        private void set_signal()
        {
            Signal signal1 = this.controller.IOSystem.GetSignal("di2");
            DigitalSignal diSig = (DigitalSignal)signal1;
            diSig.Set();
        }

        /*
          Name:  button1_Click
          Note:  单击确定按钮move到新目标点
          Date：  2017年1月4日 14:12:07
          Author: YANKE
       */

        private void button1_Click(object sender, EventArgs e)
        {
            this.write_robTarget();
            //this.set_signal();
            MessageBox.Show("改变路径");
        }

        /*
         Name:  text_changed
         Note:  文本框数字改变时弹出警告
         Date：  2017年1月4日 14:12:07
         Author: YANKE
      */
        private void text_changed(object sender, EventArgs e)
        {
            TextBox textBox = (TextBox)sender;
            check(textBox);
        }

        /*
         Name:  isTaskStart
         Note:  判断任务是否开始执行
         Date：  2017年1月4日 14:12:07
         Author: wang
      */
        private void isTaskStart(object sender, ElapsedEventArgs e)
        {
            if (this.controller != null)
            {


                this.timer1.AutoReset = false;
                this.timer1.Enabled = false;

                this.timer2.AutoReset = true;
                this.timer2.Enabled = true;

            }


        }
        /*
         Name:  ReadData
         Note:  读取并显示数据（更新速率500ms,可能还可以更快）
         Date：  2017年1月4日 14:12:07
         Author: wang
      */
        private void ReadData(object sender, ElapsedEventArgs e)
        {
            if (this.controller != null && this.tasks != null)
            {
                try
                {
                    this.textBox_c_n.Text = this.controller.Name.ToString();
                    this.textBox_c_ip.Text = this.controller.IPAddress.ToString();
                    this.textBox_c_a.Text = this.controller.AuthenticationSystem.CheckDemandGrant(controller.AuthenticationSystem.GetCurrentGrants()[0]).ToString();//控制器权限

                    this.textBox_t1.Text = this.tasks[0].Name.ToString();
                    this.textBox_t2.Text = this.tasks[0].ExecutionStatus.ToString();
                    this.textBox_t3.Text = this.tasks[0].TaskType.ToString();

                    JointTarget jt = this.tasks[0].GetJointTarget();
                    RobTarget rt = this.tasks[0].GetRobTarget();


                    this.textBox_p_x.Text = rt.Trans.X.ToString();
                    this.textBox_p_y.Text = rt.Trans.Y.ToString();
                    this.textBox_p_z.Text = rt.Trans.Z.ToString();

                    this.textBox_q1.Text = rt.Rot.Q1.ToString();
                    this.textBox_q2.Text = rt.Rot.Q2.ToString();
                    this.textBox_q3.Text = rt.Rot.Q3.ToString();
                    this.textBox_q4.Text = rt.Rot.Q4.ToString();

                    this.textBox_a1.Text = jt.RobAx.Rax_1.ToString();
                    this.textBox_a2.Text = jt.RobAx.Rax_2.ToString();
                    this.textBox_a3.Text = jt.RobAx.Rax_3.ToString();
                    this.textBox_a4.Text = jt.RobAx.Rax_4.ToString();
                    textBox_a5.Text = jt.RobAx.Rax_5.ToString();
                    this.textBox_a6.Text = jt.RobAx.Rax_6.ToString();

                    textBox_v.Text = (controller.MotionSystem.SpeedRatio * 2).ToString() + "mm/s";
                }
                catch (Exception)
                {
                    this.timer1.AutoReset = true;
                    this.timer1.Enabled = true;

                    this.timer2.AutoReset = false;
                    this.timer2.Enabled = false;
                }
            }
        }



        private void EventlogShow(object sender, MessageWrittenEventArgs e)
        {
            EventLogMessage msg = e.Message;

            this.richTextBox1.AppendText(this.controller.SystemName.ToString() + " 工作站： " + msg.Title.ToString() + "       " + msg.Timestamp.ToString() + "\r\n");
        }





        /*
        Name:  button_Click
        Note:  步进模式控制响应
        Date：  2017年1月5日 13:26:07
        Author: YANKE
     */
        private void button_Click(object sender, EventArgs e)
        {
            decimal delta = this.numericUpDown1.Value;
            Button button = (Button)sender;

            if (button.Name.Equals("up_button"))//按向上按钮
            {
                try
                {
                    RobTarget robTarget = this.tasks[0].GetRobTarget();
                    robTarget.Trans.Z = robTarget.Trans.Z + (float)delta;
                    write_robTarget(robTarget);
                }
                catch (NullReferenceException)
                {
                    MessageBox.Show("请执行任务");
                }
            }

            if (button.Name.Equals("down_button"))//按向下按钮
            {
                try
                {
                    RobTarget robTarget = this.tasks[0].GetRobTarget();
                    robTarget.Trans.Z = robTarget.Trans.Z - (float)delta;
                    write_robTarget(robTarget);
                }
                catch (NullReferenceException)
                {
                    MessageBox.Show("请执行任务");
                }
            }

            if (button.Name.Equals("right_button"))//按向右按钮
            {
                try
                {
                    RobTarget robTarget = this.tasks[0].GetRobTarget();
                    robTarget.Trans.Y = robTarget.Trans.Y + (float)delta;
                    write_robTarget(robTarget);
                }
                catch (NullReferenceException)
                {
                    MessageBox.Show("请执行任务");
                }
            }

            if (button.Name.Equals("left_button"))//按向左按钮
            {
                try
                {
                    RobTarget robTarget = this.tasks[0].GetRobTarget();
                    robTarget.Trans.Y = robTarget.Trans.Y - (float)delta;
                    write_robTarget(robTarget);
                }
                catch (NullReferenceException)
                {
                    MessageBox.Show("请执行任务");
                }
            }

            if (button.Name.Equals("forward_button"))//按向前按钮
            {
                try
                {
                    RobTarget robTarget = this.tasks[0].GetRobTarget();
                    robTarget.Trans.X = robTarget.Trans.X + (float)delta;
                    write_robTarget(robTarget);
                }
                catch (NullReferenceException)
                {
                    MessageBox.Show("请执行任务");
                }
            }

            if (button.Name.Equals("back_button"))//按后退按钮
            {
                try
                {
                    RobTarget robTarget = this.tasks[0].GetRobTarget();
                    robTarget.Trans.X = robTarget.Trans.X - (float)delta;
                    write_robTarget(robTarget);
                }
                catch (NullReferenceException)
                {
                    MessageBox.Show("请执行任务");
                }
            }
        }








        private void button_tcp_s_Click(object sender, EventArgs e)
        {
            IPAddress local = IPAddress.Parse("192.168.125.16");
            IPEndPoint iep = new IPEndPoint(local, 13000);
            //创建服务器的socket对象
            Socket server = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
            try
            {
                server.Bind(iep);
                server.Listen(20);
                server.BeginAccept(new AsyncCallback(AcceptCallback), server);
                this.richTextBox2.AppendText("服务端开启 \r\n");
            }
            catch (SocketException)
            {
                MessageBox.Show("已开启");
            }


        }

        private void AcceptCallback(IAsyncResult ar)
        {
            Socket MyServer = (Socket)ar.AsyncState;
            this.serverSocket = MyServer.EndAccept(ar);

            this.richTextBox2.AppendText("新连接接入");
            MyServer.BeginAccept(new AsyncCallback(AcceptCallback), MyServer);
            try
            {
                serverSocket.BeginReceive(buffer, 0, buffer.Length, 0, new AsyncCallback(ReceiveCallback), serverSocket);

            }
            catch (Exception e)
            {
                Console.WriteLine(e.ToString());
            }
        }

        public class UBinder : SerializationBinder
        {
            public override Type BindToType(string assemblyName, string typeName)
            {
                Assembly ass = Assembly.GetExecutingAssembly();
                return ass.GetType(typeName);
            }
        }
        //接收数据
        private void ReceiveCallback(IAsyncResult ar)
        {
            Console.WriteLine(System.DateTime.Now.ToString("s" + "yyyy-MM-dd hh:mm:ss fff"));
            try
            {

                Socket client = (Socket)ar.AsyncState;

                int bytesRead = client.EndReceive(ar);

                CmdObj cmdobj = new CmdObj(0, 0, 0, false);
                using (MemoryStream ms = new MemoryStream(buffer))
                {
                    IFormatter iformatter = new BinaryFormatter();
                    iformatter.Binder = new UBinder();
                    cmdobj = (CmdObj)iformatter.Deserialize(ms);




                    if (this.controller != null && this.tasks != null)
                    {

                        List<RobTarget> listofrobtarget = new List<RobTarget>();

                        RapidData rd = this.controller.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "wanted_pos");
                        RapidData isGrip = this.controller.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "Grip_");



                        RapidData rd_pPicktoMove = this.controller.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "pPick_toMove");

                        //初始位置值
                        RobTarget rt = (RobTarget)rd.Value;

                        //根据手势对当前坐标进行操作，得到下一目标点，并进行安全监测。
                        RobTarget rt_t = SafeTest(rt, cmdobj);


                        Boolean grab = cmdobj.Grab;





                        if (rd.Value is ABB.Robotics.Controllers.RapidDomain.RobTarget)
                        {
                            using (Mastership m = Mastership.Request(controller.Rapid))//注意这个using语句，与释放权限相关
                            {

                                try
                                {


                                    if (grab)
                                    {

                                        if (!grab_ed)
                                        {
                                            rd_pPicktoMove.Value = this.grabprocess.getPick(rt);
                                            isGrip.Value = new Num(1);
                                            grab_ed = true;
                                        }
                                        else
                                        {
                                            rd.Value = rt_t;
                                        }
                                    }
                                    else
                                    {
                                        if (grab_ed)
                                        {
                                            rd_pPicktoMove.Value = this.grabprocess.getLeave(rt);
                                            isGrip.Value = new Num(0);
                                            grab_ed = false;
                                        }
                                        else
                                        {

                                            rd.Value = rt_t;
                                        }
                                    }

                                }
                                catch (InvalidOperationException)
                                {
                                    this.richTextBox2.AppendText(this.controller.IsMaster.ToString());
                                }

                            }
                        }
                    }
                }


                // Get the rest of the data.     
                client.BeginReceive(buffer, 0, buffer.Length, 0, new AsyncCallback(ReceiveCallback), client);
            }


            catch (Exception)
            {
                this.richTextBox2.AppendText("断开");
            }
            Console.WriteLine(System.DateTime.Now.ToString("e" + "yyyy-MM-dd hh:mm:ss fff"));
        }

        private RobTarget SafeTest(RobTarget rt, CmdObj cmdobj)
        {

            float x = cmdobj.X;
            float y = cmdobj.Y;
            float z = cmdobj.Z;


            rt.Trans.X += z * 300;
            rt.Trans.Y += x * 300;
            rt.Trans.Z += y * 300;

            if (rt.Trans.Z > 900)
            {
                rt.Trans.Z = 900;
            }
            else if (rt.Trans.Z < 200)
            {
                rt.Trans.Z = 200;
            }

            if ((System.Math.Sqrt(Math.Pow(rt.Trans.X, 2) + Math.Pow(rt.Trans.Y, 2))) > 600)
            {
                rt.Trans.X -= z * 300;
                rt.Trans.Y -= x * 300;
            }

            if ((System.Math.Sqrt(Math.Pow(rt.Trans.X, 2) + Math.Pow(rt.Trans.Y, 2))) < 240)
            {
                rt.Trans.X -= z * 300;
                rt.Trans.Y -= x * 300;
            }

            return rt;
        }



        public class GrabProcess
        {

            public List<RobTarget> Grable;
            public List<RobTarget> li;
            public RobTarget Grable_ed;
            public int[] indexOfObj;

            public GrabProcess(Controller c)
            {
                this.indexOfObj = new int[] { 1, 1, 0, 0, 0 };

                this.Grable = new List<RobTarget>();
                this.li = new List<RobTarget>();
                RobTarget rd_pPick101 = (RobTarget)c.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "pPick101").Value;
                RobTarget rd_pPick102 = (RobTarget)c.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "pPick102").Value;
                RobTarget rd_pPick103 = (RobTarget)c.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "pPick103").Value;
                RobTarget rd_pPick104 = (RobTarget)c.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "pPick104").Value;
                RobTarget rd_pMiddle = (RobTarget)c.Rapid.GetRapidData("T_ROB1", "Wobj_wobj0", "pMiddle").Value;
                li.Add(rd_pPick101);
                li.Add(rd_pPick102);
                li.Add(rd_pPick103);
                li.Add(rd_pPick104);
                li.Add(rd_pMiddle);
            }


            public RobTarget getPick(RobTarget rt)
            {

                for (int s = 0; s <= indexOfObj.Length - 1; s++)
                {
                    if (indexOfObj[s] == 1)
                    {
                        this.Grable.Add(li[s]);
                    }

                }



                Double distance1 = Math.Pow(rt.Trans.X - this.Grable[0].Trans.X, 2) + Math.Pow(rt.Trans.Y - this.Grable[0].Trans.Y, 2);
                Double distance2 = Math.Pow(rt.Trans.X - this.Grable[1].Trans.X, 2) + Math.Pow(rt.Trans.Y - this.Grable[1].Trans.Y, 2);

                if (distance1 < distance2)
                {

                    this.Grable_ed = this.Grable[0];

                }
                else
                {

                    this.Grable_ed = this.Grable[1];

                }


                for (int s = 0; s <= indexOfObj.Length - 1; s++)
                {
                    if (li[s].Equals(this.Grable_ed))
                    {
                        this.indexOfObj[s] = 0;
                    }

                }

                this.Grable.Clear();
                return this.Grable_ed;

            }

            public RobTarget getLeave(RobTarget rt)
            {

                for (int s = 0; s <= indexOfObj.Length - 1; s++)
                {
                    if (indexOfObj[s] == 0)
                    {
                        this.Grable.Add(li[s]);
                    }

                }

                Double distance1 = Math.Pow(rt.Trans.X - this.Grable[0].Trans.X, 2) + Math.Pow(rt.Trans.Y - this.Grable[0].Trans.Y, 2);
                Double distance2 = Math.Pow(rt.Trans.X - this.Grable[1].Trans.X, 2) + Math.Pow(rt.Trans.Y - this.Grable[1].Trans.Y, 2);
                Double distance3 = Math.Pow(rt.Trans.X - this.Grable[2].Trans.X, 2) + Math.Pow(rt.Trans.Y - this.Grable[2].Trans.Y, 2);
                Double distance4 = Math.Pow(rt.Trans.X - this.Grable[3].Trans.X, 2) + Math.Pow(rt.Trans.Y - this.Grable[3].Trans.Y, 2);
                int i = 0;
                Double distance_temp = distance1;
                if (distance_temp > distance2)
                {
                    distance_temp = distance2;
                    i = 1;
                }
                if (distance_temp > distance3)
                {
                    distance_temp = distance3;
                    i = 2;
                }
                if (distance_temp > distance4)
                {
                    distance_temp = distance4;
                    i = 3;
                }
                this.Grable_ed = this.Grable[i];


                for (int s = 0; s <= indexOfObj.Length - 1; s++)
                {
                    if (li[s].Equals(this.Grable_ed))
                    {
                        this.indexOfObj[s] = 1;
                    }

                }
                this.Grable.Clear();


                return this.Grable_ed;
            }

        }
    }
}

       